{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "import serial\n",
    "import time\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "import os\n",
    "import sys"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "COM1 - 通信端口 (COM1)\n",
      "COM3 - USB-SERIAL CH340 (COM3)\n",
      "COM6 - 蓝牙链接上的标准串行 (COM6)\n",
      "COM8 - 蓝牙链接上的标准串行 (COM8)\n"
     ]
    }
   ],
   "source": [
    "# list all COM serial ports available on the system\n",
    "import serial.tools.list_ports\n",
    "ports = list(serial.tools.list_ports.comports())\n",
    "for p in ports:\n",
    "    print(p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Serial port is open\n",
      "b'$DGT:1-1,0!\\r\\n'\n",
      "b'{G0001#000P1500T1500!#001P2200T1500!#002P2500T1500!#003P2000T1500!#004P1500T1500!#005P1500T1500!}1500setup_PS2_success\\r\\n'\n",
      "b'[FinishInit]\\r\\n'\n"
     ]
    }
   ],
   "source": [
    "SER_COM= 'COM3' #Serial port COM number\n",
    "BAUD_RATE = 115200 #Serial port baud rate\n",
    "\n",
    "ser= serial.Serial(SER_COM, BAUD_RATE, timeout=5) #Open serial port\n",
    "\n",
    "if ser.is_open:\n",
    "    print(\"Serial port is open\")\n",
    "    while True:\n",
    "        data = ser.readline() #Read data from serial port\n",
    "        if data.find(b\"[Finish\") != -1: #If data contains [Finish] string, break the loop\n",
    "            break\n",
    "        print(data) #Print data to console\n",
    "# Last Read\n",
    "print(data)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def km_move(ser,x=0,y=200,z=100,act_time=1000):\n",
    "# 运动学测试\n",
    "    print(\"x=\",x,\"y=\",y,\"z=\",z)\n",
    "    control_cmd=\"$KMS:{x},{y},{z},{act_time}!\".format(x=x,y=y,z=z,act_time=act_time)\n",
    "    ser.write(control_cmd.encode())\n",
    "    resp=ser.read_all()\n",
    "    if ser and ser!= None:\n",
    "        print(resp.decode())\n",
    "    return resp.decode()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [],
   "source": [
    "def send_cmd(ser, cmd):\n",
    "    ser.write(cmd.encode('utf-8'))\n",
    "    time.sleep(0.2)\n",
    "    return ser.read_all().decode('utf-8')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [],
   "source": [
    "def forward(ser,speed=0,time=1000):\n",
    "    cmd_txt=\"{\"\n",
    "    for i in range(4):\n",
    "        motor_number=i+6\n",
    "        if motor_number%2==0:\n",
    "            pwm=int(speed+1500)\n",
    "        else:\n",
    "            pwm=int(-speed+1500)\n",
    "        cmd_txt+=\"#{:03d}P{:04d}T{:04d}!\".format(motor_number,pwm,time)\n",
    "    cmd_txt+=\"}\"\n",
    "    send_cmd(ser,cmd_txt)\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "forward(ser, 100,time=500)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'#006P1500T1000!'"
      ]
     },
     "execution_count": 42,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "send_cmd(ser,\"#006P1500T1000!\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x= 50 y= 200 z= 100\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n"
     ]
    }
   ],
   "source": [
    "km_move(ser=ser,\n",
    "        x=50,\n",
    "        y=200,\n",
    "        z=100,\n",
    "        act_time=1000,)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x= 0 y= 209 z= 100\n",
      "Try to find best pos:\n",
      "#005P1600T1000!#005P1500T1000!#005P1400T1000!#005P1600T1000!#005P1400T1000!#005P1600T1000!#005P1500T1000!\n",
      "x= 0 y= 209 z= 77\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 68\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 75\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 73\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 72\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 216 z= 72\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 72\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 71\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 70\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 69\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 68\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 67\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 67\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 186 z= 67\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 156 z= 67\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 156 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 156 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 155 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 135 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 156 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 190 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 190 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 190 z= 113\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 113\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 194 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 199 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 11 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 47 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 47 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 122 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 62 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 36 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 162 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 77\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 78\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 197\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 90\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 60\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 79\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 207 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 246 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 188 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 137 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 83 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 113 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 173 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 154 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 162 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 190 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 203 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 66\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 209 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 179 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 179 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 30 y= 179 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 92 y= 179 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 4 y= 179 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 4 y= 88 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 4 y= 88 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 4 y= 126 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 4 y= 147 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 4 y= 199 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 199 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 0 z= 58\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 0 z= 0\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 0 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 45 z= 141\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 141\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 137 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 222 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 222 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 353 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 286 z= 141\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 323 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 293 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 254 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 216 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 139 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 111 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 75 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 75 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 4 z= 141\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 4 z= 222\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 4 z= 267\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 4 z= 280\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 4 z= 160\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 56 z= 160\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 57 z= 160\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 58 z= 160\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 160\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 199\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 301\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 440\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 209\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 186\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 167\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 212\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 224\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 241\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 259\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 280\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 297\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 115 z= 291\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 282\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 115 z= 276\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 130 z= 276\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 103 z= 276\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 103 z= 284\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 103 z= 314\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 60 z= 314\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 51 z= 314\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 51 z= 327\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 51 z= 350\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 51 z= 361\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 77 z= 361\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 77 z= 372\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 77 z= 280\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 77 z= 244\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 77 z= 250\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 77 z= 269\n",
      "Try to find best pos:\n",
      "\n",
      "x= 0 y= 77 z= 278\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 77 z= 201\n",
      "Try to find best pos:\n",
      "Can not find best pos!!!\n",
      "\n",
      "x= 0 y= 122 z= 201\n",
      "Try to find best pos:\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# 创建图形界面\n",
    "import tkinter as tk\n",
    "\n",
    "# 创建窗口\n",
    "window = tk.Tk()\n",
    "\n",
    "# 设置窗口标题\n",
    "window.title(\"Hello World\")\n",
    "\n",
    "# 添加三个标签和拖动条，分别调节x、y、z数值，限制为0到1000\n",
    "\n",
    "import tkinter as tk\n",
    "\n",
    "import tkinter as tk\n",
    "\n",
    "x_label = tk.Label(window, text=\"X:\")\n",
    "x_label.pack()\n",
    "x_scale = tk.Scale(window, from_=0, to=1000, orient=tk.HORIZONTAL, length=500)\n",
    "x_scale.pack()\n",
    "x_scale.set(0)  # 设置默认值为0\n",
    "\n",
    "y_label = tk.Label(window, text=\"Y:\")\n",
    "y_label.pack()\n",
    "y_scale = tk.Scale(window, from_=0, to=1000, orient=tk.HORIZONTAL, length=500)\n",
    "y_scale.pack()\n",
    "y_scale.set(200)  # 设置默认值为200\n",
    "\n",
    "z_label = tk.Label(window, text=\"Z:\")\n",
    "z_label.pack()\n",
    "z_scale = tk.Scale(window, from_=0, to=1000, orient=tk.HORIZONTAL, length=500)\n",
    "z_scale.pack()\n",
    "z_scale.set(100)  # 设置默认值为100\n",
    "\n",
    "#添加一个标签输出返回值\n",
    "result_label = tk.Label(window, text=\"Result:\")\n",
    "result_label.pack()\n",
    "\n",
    "# 定义一个函数，用于计算并输出当前的x、y、z数值\n",
    "def calculate(event):\n",
    "    x = x_scale.get()\n",
    "    y = y_scale.get()\n",
    "    z = z_scale.get()\n",
    "    result = x + y + z\n",
    "    result_label.config(text=\"Result: \" + str(result))\n",
    "\n",
    "# 定义一个函数，用于更新标签显示当前的x、y、z数值\n",
    "def update_label(event):\n",
    "    x = x_scale.get()\n",
    "    y = y_scale.get()\n",
    "    z = z_scale.get()\n",
    "    x_label.config(text=\"X: \" + str(x))\n",
    "    y_label.config(text=\"Y: \" + str(y))\n",
    "    z_label.config(text=\"Z: \" + str(z))\n",
    "    result=km_move(ser,x,y,z)\n",
    "    result_label.config(text=str(result))\n",
    "    \n",
    "\n",
    "# 绑定鼠标左键松开事件，用于更新标签显示当前的x、y、z数值\n",
    "x_scale.bind(\"<ButtonRelease-1>\", update_label)\n",
    "y_scale.bind(\"<ButtonRelease-1>\", update_label)\n",
    "z_scale.bind(\"<ButtonRelease-1>\", update_label)\n",
    "\n",
    "# 启动窗口\n",
    "window.mainloop()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [],
   "source": [
    "ser.close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
